# 2023年7月29日09:17:36

# Edge Impulse - OpenMV Object Detection Example

# 视觉部分实现

#导入-----------------------------------------------------------------------------------------------
import sensor, image, time, os, tf, math, uos, gc
import lcd              #LCD显示屏幕
import json
from pyb import UART    #串口



#初始化设置
sensor.reset()                          # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565)     # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA2)     # Set frame size to QQVGA2 (128x160)
#sensor.set_windowing((240, 240))       # Set 240x240 window.
sensor.skip_frames(time=2000)           # Let the camera adjust.
lcd.init() # Initialize the lcd screen.
uart = UART(3, 115200)                  #串口初始化    波特率-115200




#声明变量--------------------------------------------------------------------------------------------
net = None                              #要导入的模型文件，先设置为空
labels = None                           #要导入的标签文件，先设置为空
min_confidence = 0.5                    #最小置信度 0.5
Mode_Flag = 1
#模式切换--------------------------------------------------------------------------------------------
#模式1：开机识别数字
#模式2：巡线
#模式3：再次识别数字，发送左转or右转指令
#模式4：远端
#---------------------------------------------------------------------------------------------------

#巡线ROI区域，上半屏幕的4个框
Rectangle_ROI1 = (0,  5, 128, 16)
Rectangle_ROI2 = (0, 25, 128, 16)
Rectangle_ROI3 = (0, 45, 128, 16)
Rectangle_ROI4 = (0, 65, 128, 16)
red_thresholds = (47, 0, 78, 11, 65, -31)   #红色线阈值

#声明变量--------------------------------------------------------------------------------------------
max_blob = 0
redline_rec1 = 0
redline_rec2 = 0
redline_rec3 = 0
redline_rec4 = 0
Start_Num = 0   #起始识别的数字
cross_stop_flag = 0   #第二个框识别红色色块，十字 T形路口停车标志位

Double_Num_left  = 0    #第二次识别到的数字，左边
Double_Num_right = 0    #

jjj = 0
Number_array = [0, 0, 0, 0, 0, 0, 0, 0]

#导入模型--------------------------------------------------------------------------------------------
try:
    # load the model, alloc the model file on the heap if we have at least 64K free after loading
    net = tf.load("trained.tflite", load_to_fb=uos.stat('trained.tflite')[6] > (gc.mem_free() - (64*1024)))
except Exception as e:      #加载异常的报错Failed to load
    raise Exception('Failed to load "trained.tflite", did you copy the .tflite and labels.txt file onto the mass-storage device? (' + str(e) + ')')
#导入列表
try:
    # 对列表labels进行赋值，列表内容是labels.txt内每一行的内容，以换行符分割元素
    labels = [line.rstrip('\n') for line in open("labels.txt")]
except Exception as e:
    raise Exception('Failed to load "labels.txt", did you copy the .tflite and labels.txt file onto the mass-storage device? (' + str(e) + ')')

colors = [ # Add more colors if you are detecting more than 7 types of classes at once.
    (  0,   0,   0),    #白色 background
    (255,   0,   0),    #红色 1
    (  0, 255,   0),    #绿色 2
    (  0,   0, 255),    #蓝色 3
    (255, 255,   0),    #黄色 4
    (255,   0, 255),    #品红 5
    (  0, 255, 255),    #青色 6
    (255, 255, 255),    #
    (230,  25, 180),    #多写两行，解决报错 ErrorIndex
]

#找最大的小球，一般认为是同样大小的小球，色块最大，即距离最近的小球
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob
#-------------------------------------------------------


# 数字识别的提示 detect() returns all objects found in the image (splitted out per class already)
# we skip class index 0, as that is the background, and then draw circles of the center of our objects

clock = time.clock()

#while(1)-------------------------------------------------------------------------------------------
while(True):
    clock.tick()

    img = sensor.snapshot()     #拍照

    # 1.串口接收数据，处理 Mode_Flag 标志位

#上电初始化，发呆，等待接收数字识别的指令
    if (Mode_Flag == 0):
        img.draw_line((64, 0, 64, 160), color = 255)        #划屏幕中线
        print("Mode0：Please put the number")    #提示放置数字

        #等待 串口接收到 数字识别的指令?  0xAA 0x00 0xFF
        if uart.any():
            Rec_string = uart.readline().decode()   #串口接收数据    decode() 应该是删除b，格式转换的意思
            print(Rec_string)   #在串行终端中显示出来接收到的字符串

            if (Rec_string == "AA 00 FF"):
                Mode_Flag = 1           #切换成 模式1 数字识别
                print("Mode1：Identify the number")

                #uart.write(Rec_string)
        #收到指令，切换为模式1



#如果是模式1，开机识别数字------------------------------------------------------------------------------
    if (Mode_Flag == 1):
        # 数字识别
        for i, detection_list in enumerate(net.detect(img, thresholds=[(math.ceil(min_confidence * 255), 255)])):
            if (i == 0): continue # background class
            if (len(detection_list) == 0): continue # no detections for this class?

            #计算识别到的数字的中心xy坐标
            for d in detection_list:
                [x, y, w, h] = d.rect()
                center_x = math.floor(x + (w / 2))
                center_y = math.floor(y + (h / 2))
            #将识别到的数字标记在LCD上
            if (center_x <= 64):    # 中心坐标x <= 64,识别的图像在左边
                img.draw_string(  0, 0, labels[i], color = 130, scale = 4)
            if (center_x >= 64):    # 中心坐标x >= 64,识别的图像在右边
                img.draw_string(100, 0, labels[i], color = 130, scale = 4)
            #注意对齐格式，串口发送数据写在这里！
            #print("********** %s **********" % labels[i])               #显示识别出来的数字
            #Prescription_Num = '0x{:02X}'.format( (int)(labels[i]) )    #转化药房号码，变成0x0？ 这句只是处理成0x0？  但是不能发送
            Prescription_Num = (int)(labels[i])     #转化药房号码
            Start_Num = Prescription_Num    #将药房号码赋值给 起始识别的数字(这是个全局变量)
            # print('Prescription Num = ', Prescription_Num)    #输出药房号码数据

            # 识别10次，统计 返回最大的那个数据
            Number_array[jjj] = Prescription_Num    #把每次识别到的数字，都赋值给这个数组

            if (jjj < 6):
                jjj = jjj + 1                              #数组索引++
            if (jjj == 6):     #如果识别了5次数字了
                Max_appear_result = max(set(Number_array), key=Number_array.count)      #判断出现次数最多的那个数字
                print(Number_array)
                print('Max_appear_result = ', Max_appear_result)   #输出统计计算后的药房号码数据
                #发送药房号 串口发送数据 0xAA 0x00 0x0? 0xFF        数据0x0? --- 药房是几 ?就是几
                FH = bytearray([0xAA, 0x00, Prescription_Num, 0xFF]) #巡线
                uart.write(FH)
                jjj = jjj + 1
                #这段代码实现，只发送一次识别到的数据
        #模式1内 识别完数字，统计完出现最多的数字，发送给单片机后，一直等待 接收到巡线指令，才跳转到模式2，因为只有openmv发送了药房号码，单片机才发送寻迹指令
        if uart.any():
            Rec_string = uart.readline().decode()   #串口接收数据
            #print(Rec_string)  #打印返回值
            if (Rec_string == "AA 01 FF"):
                Mode_Flag = 2           #切换成 模式2 巡线



#模式2 巡线------------------------------------------------------------------------------------------
    if (Mode_Flag == 2):
        cross_stop_flag = 0     #先把路口停车标志位置0 不停车，再次进入巡线模式，清除之前的状态
        #画四个框 前期画框确定roi区域，后续去掉，提高帧率
        #img.draw_rectangle(Rectangle_ROI1, color = (255,255,255))
        #img.draw_rectangle(Rectangle_ROI2, color = (255,255,255))
        #img.draw_rectangle(Rectangle_ROI3, color = (255,255,255))
        #img.draw_rectangle(Rectangle_ROI4, color = (255,255,255))


        #这四个标志位是用来判断药房停车的，如果检测不到(默认)，就置0，如果检测到红线，标志位置1，
        blobline1_flag = 0
        blobline2_flag = 0
        blobline3_flag = 0
        blobline4_flag = 0

        #识别色块-方法3--------------------------------------------------------------
        #识别色块1
        blobs = img.find_blobs([red_thresholds], roi = Rectangle_ROI1)      #颜色识别
        if blobs:
            max_blob = find_max(blobs)                                      #寻找最大色块
            img.draw_rectangle(max_blob.rect())                             #画矩形
            img.draw_cross(max_blob.cx(), max_blob.cy())                    #画十字
            redline_rec1 = max_blob.cx()                                    #记录巡线色块x中心坐标
            blobline1_flag == 1     #判断药房停车的标志位

        #识别色块2
        blobs = img.find_blobs([red_thresholds], roi = Rectangle_ROI2)      #颜色识别
        if blobs:
            max_blob = find_max(blobs)                                      #寻找最大色块
            img.draw_rectangle(max_blob.rect())                             #画矩形
            img.draw_cross(max_blob.cx(), max_blob.cy())                    #画十字
            redline_rec2 = max_blob.cx()                                    #记录巡线色块x中心坐标
            blobline2_flag = 1  #判断药房停车的标志位

            #判断 十字路口 T形路口，   判断第二个色块中，识别到的像素 >= 500,认为是十字路口，可以停车了
            if (max_blob[2]*max_blob[3] >= 500):        #长*宽 >= 500
                cross_stop_flag = 1   #停车标志位置1


        #识别色块3
        blobs = img.find_blobs([red_thresholds], roi = Rectangle_ROI3)      #颜色识别
        if blobs:
            max_blob = find_max(blobs)                                      #寻找最大色块
            img.draw_rectangle(max_blob.rect())                             #画矩形
            img.draw_cross(max_blob.cx(), max_blob.cy())                    #画十字
            redline_rec3 = max_blob.cx()
            blobline3_flag = 1

        #识别色块4
        blobs = img.find_blobs([red_thresholds], roi = Rectangle_ROI4)      #颜色识别
        if blobs:
            max_blob = find_max(blobs)                                      #寻找最大色块
            img.draw_rectangle(max_blob.rect())                             #画矩形
            img.draw_cross(max_blob.cx(), max_blob.cy())                    #画十字
            redline_rec4 = max_blob.cx()
            blobline4_flag = 1
        #计算距离偏差 没有取平均值(浮点数)，直接发送偏差之和
        Distance_Error = ((redline_rec1-64) + (redline_rec2-64) + (redline_rec3-64) + (redline_rec4)-64)

        #这里可以优化一下，把这句if放到 识别色块2 和 识别色块3中间，这样会跳过 识别色块3 4更快速的发送停车指令，这里发送快了，是不是需要重新调参？？？暂时不提前位置了
        if (cross_stop_flag == 1):    #如果第二个方块检测到十字，停车
            #发送停车指令
            #巡线 串口发送数据 0xAA 0x02 0xFF 0xFF
            FH = bytearray([0xAA, 0x02, 0xFF, 0xFF])             #停车
            uart.write(FH)                              #发送 0xAA 0x02 0xFF 0xFF
            print("cross stop")
        #print(cross_stop_flag)

        #redline_rec1 = 64       #置偏移x坐标64    变量 redline_rec1-64 = 0，完成清零
        #redline_rec2 = 64       #如果识别不到目标，就保持上一次发送的偏差数据，强制纠正小车，就不需要清零
        #redline_rec3 = 64
        #redline_rec4 = 64

        #等待 接收到 识别数字 指令，才跳转到模式3
        if uart.any():
            Rec_string = uart.readline().decode()   #串口接收数据
            #print(Rec_string)
            if (Rec_string == "AA 02 FF"):
                Mode_Flag = 3           #切换成 模式3 路口识别
            elif(Rec_string == "AA 03 FF"): #在巡线模式2中，第三个路口，先发送了停车指令，等待F4判断是第三个路口，openmv接收模式4，进行远端识别
                Mode_Flag = 4           #切换成 模式4 远端模式

        #如果是药房虚线检测，四个色块都检测不到红线，发停车指令，进入库房
        if ( (blobline1_flag == 0) and (blobline2_flag == 0) and (blobline3_flag == 0) and (blobline4_flag == 0) ):
            #发送停车指令     #巡线 串口发送数据 0xAA 0x02 0xFF 0xFF
            FH = bytearray([0xAA, 0x02, 0xFF, 0xFF])             #停车
            uart.write(FH)                              #发送 0xAA 0x02 0xFF 0xFF
            print("stop")
        else:   #只要4个色块中有能识别到红线的，就发送巡线的偏差数据
            #巡线 串口发送数据 0xAA 0x01 output_str 0xFF
            FH = bytearray([0xAA,0x01]) #巡线
            uart.write(FH)                          #发送 0xAA 0x01
            Output_Distance = "%d" % (Distance_Error)
            uart.write(Output_Distance)                  #发送 数据 距离
            FH = bytearray([0xFF])
            uart.write(FH)                          #发送 0xFF
            print(Distance_Error)                   #电脑串行终端显示距离



#如果是模式3，路口再次识别数字，发送左转or右转指令----------------------------------------------------------
    if (Mode_Flag == 3):    #模式3
        cross_stop_flag = 0       #将模式2的停车标志位置0

        # 转弯之后 + 直行之后，收到 继续巡线的指令，切换成模式2
        if uart.any():
            Rec_string = uart.readline().decode()   #串口接收数据
            #print(Rec_string)
            if (Rec_string == "AA 01 FF"):
                Mode_Flag = 2           #切换成 模式2 巡线

        # 1号路线 2号路线 没有数字识别
        if ((Start_Num == 1) or (Start_Num == 2)):  #如果起始识别到的库房 是1号库或2号库
            if (Start_Num == 1):
            #1号库 发送左转指令
                FH = bytearray([0xAA, 0x03, 0x03, 0xFF]) #0x03数据类型  数据0x03 左转
                uart.write(FH)
            if (Start_Num == 2):
            #2号库 发送右转指令
                FH = bytearray([0xAA, 0x03, 0x02, 0xFF]) #0x03数据类型  数据0x02 右转
                uart.write(FH)

        # 3~8号库
        else:   #如果是3~8号库
            #进行数字识别
            for i, detection_list in enumerate(net.detect(img, thresholds=[(math.ceil(min_confidence * 255), 255)])):
                if (i == 0): continue # background class
                if (len(detection_list) == 0): continue # no detections for this class?
                print("********** %s **********" % labels[i])

                #计算中心坐标
                for d in detection_list:
                    [x, y, w, h] = d.rect()
                    center_x = math.floor(x + (w / 2))
                    center_y = math.floor(y + (h / 2))
                    print('x %d\ty %d' % (center_x, center_y))
                if (center_x <= 64):    # 中心坐标x <= 64,左边识别的图像
                    img.draw_string(  0, 0, labels[i], color = 130, scale = 4)
                    Double_Num_left = (int)(labels[i])     #再次识别到 左边的数字
                    #if (Start_Num == Double_Num_right):
                    #发送左转指令


                if (center_x >= 64):    # 中心坐标x >= 64,在右边识别的图像
                    img.draw_string(100, 0, labels[i], color = 130, scale = 4)
                    Double_Num_right = (int)(labels[i])     #再次识别到 右边的数字
                    #if (Start_Num == Double_Num_right):
                    #发送右转指令

            #提高了一个优先级 识别之后就一直判断，而不是只有识别过程中才判断，
            if (Start_Num == Double_Num_left):     #如果识别到起始数字和左边的数字相同，那就发送左转
                FH = bytearray([0xAA, 0x03, 0x03, 0xFF]) #0x03数据类型  数据0x03 左转
                uart.write(FH)
                print("tern left")
            elif(Start_Num == Double_Num_right):    #如果识别到起始数字和右边的数字相同，那就发送右转
                FH = bytearray([0xAA, 0x03, 0x02, 0xFF]) #0x03数据类型  数据0x02 右转
                uart.write(FH)
                print("tern right")
            else:       #如果识别到的两个数字和最开始识别到的数字都不相等，那就继续直行，开往中端和远端
                #直行 串口发送
                FH = bytearray([0xAA, 0x03, 0x01, 0xFF]) #0x03数据类型  数据0x03 左转
                uart.write(FH)
                print("go on")
                #这里如果是第一个路口，没有识别到，就会发送直行，等待接收巡线指令

            if uart.any():
                Rec_string = uart.readline().decode()   #串口接收数据
                #print(Rec_string)
                if (Rec_string == "AA 01 FF"):
                    Mode_Flag = 2           #切换成 模式2 巡线
            #Prescription_Num = 1        #这里置1是什么意思？
            #img.draw_circle((center_x, center_y, 12), color=colors[i], thickness=2)
            #这里串口发送指令，
        if uart.any():
            Rec_string = uart.readline().decode()   #串口接收数据
            #print(Rec_string)
            if (Rec_string == "AA 01 FF"):
                Mode_Flag = 2           #切换成 模式2 巡线



#模式4 远端病房
    if (Mode_Flag == 4):    #模式4
        #进行数字识别
        for i, detection_list in enumerate(net.detect(img, thresholds=[(math.ceil(min_confidence * 255), 255)])):
            if (i == 0): continue # background class
            if (len(detection_list) == 0): continue # no detections for this class?
            print("********** %s **********" % labels[i])
            #计算中心坐标
            for d in detection_list:
                [x, y, w, h] = d.rect()
                center_x = math.floor(x + (w / 2))
                center_y = math.floor(y + (h / 2))
                print('x %d\ty %d' % (center_x, center_y))
            if (center_x <= 64):    # 中心坐标x <= 64,左边识别的图像
                img.draw_string(  0, 0, labels[i], color = 130, scale = 4)
                Double_Num_left = (int)(labels[i])     #再次识别到 左边的数字

            if (center_x >= 64):    # 中心坐标x >= 64,在右边识别的图像
                img.draw_string(100, 0, labels[i], color = 130, scale = 4)
                Double_Num_right = (int)(labels[i])     #再次识别到 右边的数字

        if (Start_Num == Double_Num_left):     #如果识别到起始数字和左边的数字相同，那就发送左转
                FH = bytearray([0xAA, 0x03, 0x03, 0xFF]) #0x03数据类型  数据0x03 左转
                uart.write(FH)
        if(Start_Num == Double_Num_right):    #如果识别到起始数字和右边的数字相同，那就发送右转
            FH = bytearray([0xAA, 0x03, 0x02, 0xFF]) #0x03数据类型  数据0x02 右转
            uart.write(FH)
        #else:       #如果识别到的两个数字和最开始识别到的数字都不相等，那就不发数据
            ##直行 串口发送
            #FH = bytearray([0xAA, 0x03, 0x01, 0xFF]) #0x03数据类型  数据0x03 左转
            #uart.write(FH)



#LCD显示
    lcd.display(img) # Take a picture and display the image.    #改变图像之后，显示修改最新版本的图像
    #print(clock.fps(), "fps", end="\n\n")
